#include <path_planning/path_planner.h>
#include <libplayerinterface/player.h>
#include <libplayerc++/playerc++.h>
#include <boost/thread/thread.hpp>
#include <iostream>
#include <cstdlib>

using std::cout;
using std::cin;
using std::endl;
using PlayerCc::dtor;

int main(int argc, char *argv[]) {
  player_point_2d initial_position, goal_position;
  double initial_yaw, goal_yaw;

  PlayerCc::PlayerClient player_client("localhost");
  PlayerCc::Position2dProxy position2d_proxy(&player_client);
  PathPlanner path_planner(argv[1], argv[2]);
  player_client.Read();

  cout << "Initial pose (px py pa). pa in degrees.: ";
  cin >> initial_position.px >> initial_position.py >> initial_yaw;
  initial_yaw = dtor(initial_yaw);
  cout << "Goal pose (px py pa): ";
  cin >> goal_position.px >> goal_position.py >> goal_yaw;
  goal_yaw = dtor(goal_yaw);
  position2d_proxy.SetOdometry(initial_position.px, initial_position.py, initial_yaw);
  position2d_proxy.SetMotorEnable(1);

  WaypointList *waypoint_list = path_planner.RequestWaypoints(initial_position, goal_position);
/*
      player_point_2d *waypoint = waypoint_list->Next();
     player_pose2d pose = {waypoint->px, waypoint->py, goal_yaw};
     position2d_proxy.GoTo(pose);
      while(1);*/
  player_point_2d *waypoint;
  while ((waypoint = waypoint_list->Next()) != NULL) {
     player_pose2d pose = {waypoint->px, waypoint->py, goal_yaw};
     position2d_proxy.GoTo(pose);
     while (1) {
       player_client.Read();
       double dx = waypoint->px - position2d_proxy.GetXPos();
       double dy = waypoint->py - position2d_proxy.GetYPos();
       if (dx * dx + dy * dy < 0.1) break;
       boost::this_thread::sleep(boost::posix_time::milliseconds(300)); 
     }
     cout << "WAYP:" << waypoint->px << " " << waypoint->py << endl;
     cout << "ODOM:" << position2d_proxy.GetXPos() 
          << " " << position2d_proxy.GetYPos() << endl << endl;
  }
  return 0;
}
